/**
 * Arduino两轮小车
 * 
 * 本程序可不受限制的用于学习和教学，商业用途请联系作者。
 * 本程序不对用户因使用程序而造成任何形式的损失负责。
 * 
 * site: http://www.xpstem.com/maker/arduino-vehicle
 * author: billy zhang
 */
 
#include <Arduino.h>
#include "UltrasonicVehicle.h"

#define MaxPower 255
#define MinPower 120

#define KeepAway 25//离障碍最近距离
#define LookDelay 100//转动舵机的停顿时间
#define TurnDelay 200//转向停顿时间，通过停顿时间控制转弯角度大小
#define BackDelay 200//倒退时间
#define ReboundDelay 150 //反弹时间

void UltrasonicVehicle::setup()
{
  
}

void UltrasonicVehicle::drive()
{
  int distance = lidar_.scan(90);//调整超声波探测模块向前看
  if (distance > KeepAway) {//如果距离大于最小距离
    driven_.forward(MaxPower);//一直向前
  } else {
    
    driven_.stop();
    driven_.backward(MaxPower);
    delay(ReboundDelay);
    driven_.stop();

    switch (lookAround()) {//判断观察周围情况结果，并执行相应动作
      case 1:
        driven_.turnRight(MinPower);
        delay(TurnDelay);
        driven_.stop();
        return;
      case 2:
        driven_.turnRight(MinPower);
        delay(TurnDelay / 2);
        driven_.stop();
        return;
      case 3:
        driven_.forward(MaxPower);
        return;
      case 4:
        driven_.turnLeft(MinPower);
        delay(TurnDelay / 2);
        driven_.stop();
        return;
      case 5:
        driven_.turnLeft(MinPower);
        delay(TurnDelay);
        driven_.stop();
        return;
      default:
        break;
    }
    
  }  
}

int UltrasonicVehicle::lookAround() {//观察周围情况，并返回方向代码
  int D1 = lidar_.scan(30);
  int D2 = lidar_.scan(60);
  int D3 = lidar_.scan(90);
  int D4 = lidar_.scan(120);
  int D5 = lidar_.scan(150);
  if (D1 > D2 && D1 > D3 && D1 > D4 && D1 > D5 && D1 > KeepAway)return 1;
  if (D2 > D1 && D2 > D3 && D2 > D4 && D2 > D5 && D2 > KeepAway)return 2;
  if (D3 > D2 && D3 > D1 && D3 > D4 && D3 > D5 && D3 > KeepAway)return 3;
  if (D4 > D2 && D4 > D3 && D4 > D1 && D4 > D5 && D4 > KeepAway)return 4;
  if (D5 > D2 && D5 > D3 && D5 > D4 && D5 > D1 && D5 > KeepAway)return 5;
  return 0;
}

UltrasonicVehicle::~UltrasonicVehicle()
{
}
